#include "Random.hpp"
#include "simulate/birtnorm.hpp"
#include "simulate/rtnomNC.hpp"
#include "simulate/rtnorm.hpp"
#include "simulate/src/rtnormNC.hpp"
#include <boost/timer.hpp>
//#include <R.h>
//#include <Rmath.h>
#include "Data/Data.hpp" 

template<class Resample,class Kernel>
class Ghk
{
	public:
	Ghk(Resample *R, Kernel *K){

		boost::random::uniform_01<> U;
		_runif=new RandomG::Random<boost::random::uniform_01<> >(U);
		_R=R;
		_K=K;
		d=new boost::math::normal_distribution<>(0,1); 
		rt=new rtnormNC();
		rt2=new rtnorm();
		rt3=new rtNC();
		boost::random::normal_distribution<> N2;
		_rnorm= new RandomG::Random<boost::random::normal_distribution<> >(N2);
		sigma=0.1;
	 	int _nu=5;	
		boost::random::gamma_distribution<> N3(_nu/2,2);
		rgamma= new RandomG::Random<boost::random::gamma_distribution<> >(N3);
	}
	~Ghk()
	{
		delete d;
		delete _runif;
	}

	void ghk_oneside(double *L, double* trunpt, int *above, int *dim,double *eta, int *n, double *res, int tr)
	/*	routine to implement ghk with a region defined by truncation only on one- side
							r mcculloch 8/04
		if above=1, then we truncate component i from above at point trunpt[i-1]
		L is lower triangular root of Sigma
		random vector is assumed to have zero mean
		n is number of draws to use in GHK	
		modified 6/05 by rossi to check arg into qnorm
		modified 2012 by ridgway to make it awesome!!!! 
	 */
	{
	 
		if(tr==1)
		{
			O1=new Data<ofstream>("G_Move.txt");	
		}else if(tr==2){
			O1=new Data<ofstream>("C_Move.txt");	
		}
	   	int i,j,k;
	   double mu,tpz,u,pa,pb,arg;
	   mat z(*n,*dim);
	   double *prod=new double[*n];
	   Normalize(prod,n);
	   double thres=*eta*(*n);
	   z.fill(0);
	   double *Nt=new double[(*n)*(*dim)];
	  // sobol::Owenscramble O;
	   //O(*n,*dim,0,Nt);
	   *res = 0;
	   int kk=0;
	   int b=0;
	 // mat X=vect2mat<double>(L,*dim,*dim);
	 //cout << X;
	 //mat U(1,*dim);
	 //U.fill(0);
	   double times=2.38;
	//SampleGibbs(dim,above,trunpt,L,&U); 
	      for(j=0;j<*dim;j++) {
			//Resample
		      double ess=Ess(prod,n);
		  //    cout << "ess " << ess <<"\n";
		      if(ess<thres){	
		      cout << "ess " << ess <<" ";
				*res+=mean2(prod,n);
			//	cout << mean2(prod,n); 
		      		fResample(prod,n,&z);
				b=1;
				double rho=0.8;
				//cout << "m1 "<< mean(z,0);		
				double acc=0;
		//		mat info=InfoCone(L,j,dim,trunpt);
				//while(acc<0.15 | acc>0.35)
				mat XX=z;
				if(tr==1)
				{	
						std::ostringstream oss1;
						oss1 << j;
						string sj=oss1.str();
						(*O1).Write(sj);
						(*O1).Write(" ");
						boost::timer tt;
						double dist;
						double dist2;
						double diff=2;
						if(j<400)
						{
							while(abs(diff)>0.1)
							{
								GibbsMove2(dim,n,above,trunpt,L,&z,j);
								dist=mdist(XX,z);
								diff=dist-dist2;
								cout << "dist1a " << diff;
								dist2=dist;
							}
						}else{
							double ratio;
							while(abs(diff)>0.01)
							{			//acc=Langevin(dim,n,above,trunpt,L,&z,j,times);
								ratio=GRW(dim,n,above,trunpt,L,&z,j);
								double ratio2=RandomDirect(dim,n,above,trunpt,L,&z,j,times);
								dist=mdist(XX,z);
								diff=dist-dist2;
								cout << "dist1b " << diff;
								dist2=dist;
							}

						}	//		cout << " time G " << tt.elapsed()<< " "; 
						std::ostringstream oss;
						oss << tt.elapsed();
						string s=oss.str();
						//cout << " t "<<s;
						dist=mdist(XX,z);
						std::ostringstream oss2;
						oss2 << dist;
						string s2=oss2.str();
						(*O1).Write(s);
						(*O1).Write(" ");
						(*O1).Write(s2);
						(*O1).Write("\n");
				}else if(tr==2){
				//cout << " gibbs " << mdist(XX,z);
			//	cout << "acc "<< acc ;
			//	z=XX;
				std::ostringstream oss1;
				oss1 << j;
				string sj=oss1.str();
				(*O1).Write(sj);
				(*O1).Write(" ");

				boost::timer tt2;
				//double ratio=GRW(dim,n,above,trunpt,L,&z,j,(1-sqrt(exp(*res))));
				//acc=Langevin(dim,n,above,trunpt,L,&z,j,times);
				double dist;
				double dist2;
				double diff=2;
				double ratio;
				double ratio2;
				while(abs(diff)>0.01)
				{
					//acc=RandomWalk(dim,n,above,trunpt,L,&z,j,times);
					//acc=Langevin(dim,n,above,trunpt,L,&z,j,times);
					ratio=GRW(dim,n,above,trunpt,L,&z,j);
					//ratio2=RandomDirect(dim,n,above,trunpt,L,&z,j,times);
					dist=mdist(XX,z);
					diff=dist-dist2;
					cout << "dist2 " << diff;
					dist2=dist;
				}
				//cout << " GRW " << mdist(XX,z);
				//	if(acc >0.35)
				//	{
				//		times*=1.1;
				//	}else if(acc<0.15){
				//		times*=0.9;
				//	}
				//z=XX;
			//	boost::timer tt1;
				//double ratio2=RandomDirect(dim,n,above,trunpt,L,&z,j,times);
				std::ostringstream oss;
				oss << tt2.elapsed();
				string s=oss.str();
				std::ostringstream oss2;
				oss2 << dist;
				string s2=oss2.str();
				(*O1).Write(s);
				(*O1).Write(" ");
				(*O1).Write(s2);
				(*O1).Write("\n");
			//	cout << "acceptance2 " << ratio2 << " ";
			//	cout << "acceptance2 " << ratio2 << " ";
					//cout << " time GRW " << tt2.elapsed() << " "; 
				}else{
					
					_K->Move2(&(z),j);

				}
			/*	cout << " RD " << mdist(XX,z);
				cout << " time " << tt1.elapsed(); 
				*/
				//z=XX;
			//	boost::timer tt3;
			//	cout << "HMC " << mdist(XX,z);
			//	cout << " time " << tt3.elapsed(); 
				
				
				
		//		cout << "info "<< info.t();		
				
			}
				// set weigths	      
				for(i=0;i<*n;i++) {
					 mu=0.0;for(k=0;k<j;k++) mu += L[k*(*dim)+j]*z(i,k);
					 tpz = (trunpt[j]-mu)/L[j*(*dim)+j];
					 if(above[j]){
					    pa=0.0; pb = cdf(*d,tpz);   
					 }else{
					    pb=1.0; pa = cdf(*d,tpz);
					 }
					 prod[i]*= pb-pa;
					 //cout << prod[i] << " ";
					 u = (*_runif)(); 
					 //u=Nt[kk];
					 arg=u*pb+(1.-u)*pa;
					 if(arg > .999999999) arg=.999999999;
					 if(arg < .0000000001) arg=.0000000001;
					 z(i,j) = quantile(*d,arg);
			      		 kk++;
				}
		//		cout << "\n\\\\\\\\\\\\\\\\\\\\\\\n";
	   }
		      *res+=mean2(prod,n);
	   //	cout << "res " << *res;		
	   delete[] prod;
	   O1->Close();
	}

	inline void Normalize(double *w,int *n)
	{
		for(int i=0;i<*n;i++)
		{
			w[i]=1;
		}
	}
	inline double mean2(double *w, int *n){
		double sum=log(w[0]);
		for(int i=1;i<*n;i++){
			if(w[i]!=0)
			{
				sum=log_add(log(w[i]),sum);
			}
		}
		return sum-log(*n);
	}
		
	inline void fResample(double *w, int *n,mat *z)
	{
		mat v=growingvect(*n);
		(*_R)(n,w,v);
		Arangemat(*z,v);
		Normalize(w,n);
	}
	inline double Ess(double *w, int *n)
	{
		double sum=0;
		for(int i=0;i<*n;i++)
		{
			sum+=w[i];
		}
		double sum2=0;
		for(int i=0;i<*n;i++)
		{
			sum2+=pow(w[i]/sum,2);
		}
		return (double)1/sum2;

	}
	inline void GibbsMove2(int *dim,int *n,int *above,double *trunpt,double *L, mat *z, int j)
	{
		int k;
		double mu,tpz,u,pa,pb,arg;
		double ltrunc=-numeric_limits<double>::infinity();
		double rtrunc=+numeric_limits<double>::infinity();
		double curr=0;
		mat Z(*n,*dim);
		Z=*z;
		int b1=1;
		int b2=1;
		for(int i=0;i<*n;i++)
		{
			for(int l=0;l<j;l++)
			{
				b1=1;
				b2=1;
				//calcul du max
				//cout << "new " << "\n";
					ltrunc=-numeric_limits<double>::infinity();
					rtrunc=numeric_limits<double>::infinity();
					for(int ii=l;ii<j;ii++)
					{

						if(L[l*(*dim)+ii]!=0)
						{
							double temp=trunpt[ii];
							//cout << " trunc " << trunpt[ii] << " ";
							for(int jj=0;jj<=ii;jj++)
							{
								if(jj!=l)
								{
									temp-=L[jj*(*dim)+ii]*(Z)(i,jj);
						//			cout << (*z)(i,jj) << " ";
								}
							}
						//	cout << "\n";
							//cout <<  "temp1 "<<temp << " ";
							temp=temp/L[l*(*dim)+ii];
							//cout <<  "temp2 "<<temp << " ";
							if(L[l*(*dim)+ii]<0)
							{
							//	cout << "r" ;
								//cout << "rtrunc noc "<<  temp << "\n";
								if(rtrunc>temp)
								{
									b1=0;
									rtrunc=temp;
									//cout << "rtrunc "<<  temp << "\n";
								}
							}else{
							//	cout << "l" ;
								//cout << "ltrunc noc "<<  temp << "\n";
								if(ltrunc<temp)
								{
									b2=0;
									ltrunc=temp;
									//cout << "ltrunc "<<  temp << "\n";
								}
							}
						//cout <<" div " << L[l*(*dim)+ii] << " " ;
					
						}
					}
			//	cout << "l "<<ltrunc;
			//	cout << " r "<< rtrunc<< "\n";
			//	double me=as_scalar(sum((*z)(i,span::all))-(*z)(i,l));
				if(b1)
				{
					Z(i,l)=(*rt)(0,ltrunc,pINF,1);
				}else if(b2){
					Z(i,l)=(*rt)(0,-pINF,rtrunc,1);
				}else{
					//Z(i,l)=(*rt)(0,ltrunc,rtrunc,1);
				//	cout << " l "<< ltrunc << " r " << rtrunc << " ";
					if(ltrunc<rtrunc)
					{
						Z(i,l)=(*rt3)(ltrunc,rtrunc,0,1);
					}else{

					//	cout << "oups!!"; 
					}
				//	cout << "z " << Z(i,l);
				}
				//cout << "z " << Z(i,l);
			//	cout << "\n";
			}
			//				cout << "j "<<j<< " ";
							//cout << *z;
		}
		*z=Z;
	}
	

	inline mat Proposal(mat eta, mat eps, double* L,int t,int *dim)
	{
		mat xi(t,1);
		double sum=0;
		for(int i=0;i<t;i++)
		{
			xi(i,0)=eps(0,i)*(*_rnorm)();
		}
	/*	for(int i=0;i<t;i++)
		{
			sum=0;
			for(int j=0;j<=i;j++)
			{
				sum+=L[j*(*dim)+i]*xi(j,0);
			}
			xi(i,0)=sum;
		}
		*/
		return xi.t()+eta;
	}
	inline mat Thres(double *L,mat Y, int t, int *dim){
		mat xi(t,1);
		double sum;
		for(int i=0;i<t;i++)
		{
			sum=0;
			for(int j=0;j<=i;j++)
			{
				sum+=L[j*(*dim)+i]*Y(0,j);
			}
			xi(i,0)=sum;
		}
		return xi;
	}
	inline double RandomWalk(int *dim,int *n,int *above,double *trunpt,double *L, mat *z, int j, double times)
	{
		int k;
		double mu,tpz,u,pa,pb,arg;
		double mu2,tpz2,u2,pa2,pb2;
		double curr=0;
		mat Y(*n,j);
		mat eps=times*stddev(*z,0)/j;
		bool t=true;
		double ratio=0;

		for(int i=0;i<*n;i++)
		{
			Y(i,span::all)=Proposal((*z)(i,span(0,j-1)),eps,L,j,dim);	
			t=true;
			//cout << "Y " << Y(i,span::all);
			double prod=0;
			double prod2=0;
			//check constraints
			mat thres=Thres(L,Y(i,span::all),j,dim);
			for(int k=0;k<j;k++)
			{
				//cout << thres(k,0) << " " << trunpt[k] <<"\n";
				if(thres(k,0)<trunpt[k])
				{
					t=false;				
				}				
			}

			//calculate acceptance ratio
			//cout << " result " << t;
			if(t)
			{
	      			for(int jj=0;jj<j;jj++) {
				
					//cout << "ratio "<< pdf(*d,Y(i,jj));
					prod+=log(/* (pb-pa)*/pdf(*d,Y(i,jj)));
					prod2+=log(/* (pb2-pa2)*/pdf(*d,(*z)(i,jj)));
				}
				//cout << (prod-prod2) <<"\n";
				if(log((*_runif)())<(prod-prod2))
				{
					ratio+=1;
					(*z)(i,span(0,j-1))=Y(i,span::all);
				}
			//		ratio+=1;
			}
	
		}
		return(ratio/(*n));


	}
	inline mat Proposal_L(mat eta, mat eps, double* L,int t,int *dim)
	{
		mat xi(t,1);
		double sum=0;
		for(int i=0;i<t;i++)
		{
			xi(i,0)=eps(0,i)*(*_rnorm)();
		}
	/*	for(int i=0;i<t;i++)
		{
			sum=0;
			for(int j=0;j<=i;j++)
			{
				sum+=L[j*(*dim)+i]*xi(j,0);
			}
			xi(i,0)=sum;
		}
		*/
		return xi.t()+(1-(eps%eps)/2)%eta;
	}

	double dN(mat X, mat Y, mat eps)
	{
		mat temp=(Y-(1-eps%eps/2)%X);
		mat sig=diagmat(eps%eps);
		return as_scalar(-1/2*temp*inv(sig)*temp.t());	
	}

	inline double Langevin(int *dim,int *n,int *above,double *trunpt,double *L, mat *z, int j, double times)
	{
		int k;
		double mu,tpz,u,pa,pb,arg;
		double mu2,tpz2,u2,pa2,pb2;
		double curr=0;
		mat Y(*n,j);
		mat st=stddev(*z,0);
		mat eps(1,j);
		eps.fill(0.03/pow(j,(double)1/6));/*times*st(0,span(0,j-1))*/
		bool t=true;
		double ratio=0;

		for(int i=0;i<*n;i++)
		{
			Y(i,span::all)=Proposal_L((*z)(i,span(0,j-1)),eps,L,j,dim);	
			t=true;
			//cout << "Y " << Y(i,span::all);
			double prod=0;
			double prod2=0;
			//check constraints
			mat thres=Thres(L,Y(i,span::all),j,dim);
			for(int k=0;k<j;k++)
			{
				//cout << thres(k,0) << " " << trunpt[k] <<"\n";
				if(thres(k,0)<trunpt[k])
				{
					t=false;				
				}				
			}

			//calculate acceptance ratio
			//cout << " result " << t;
			if(t)
			{
	      			for(int jj=0;jj<j;jj++) {
					//cout << "ratio "<< pdf(*d,Y(i,jj));
					prod+=log(/* (pb-pa)*/pdf(*d,Y(i,jj)));
					prod2+=log(/* (pb2-pa2)*/pdf(*d,(*z)(i,jj)));
					
				}
				//cout << (prod-prod2) <<"\n";
				if(log((*_runif)())<(prod+dN(Y(i,span::all),(*z)(i,span(0,j-1)),eps)-prod2-dN((*z)(i,span(0,j-1)),Y(i,span::all),eps)))
				{
					ratio+=1;
					(*z)(i,span(0,j-1))=Y(i,span::all);
				}
			//		ratio+=1;
			}
	
		}
		return(ratio/(*n));


	}
	inline mat findSummit(double *L, int j, double *trunpt,int *dim)
	{
		mat sol(j,1);
		double s=0;
		for(int i=0;i<j;i++)
		{
			s=trunpt[i];
			for(int k=0;k<i;k++)
			{
				s-=L[k*(*dim)+i]*sol(k,0);
			}
			sol(i,0)=s/L[i*(*dim)+i];
		}
		//cout << sol.t();
		return sol;
	}

	inline double dist(mat X, mat Y)
	{
		int n=X.n_cols;
		double d=0;
		for(int i=0;i<n;i++)
		{
			d+=((X(0,i)-Y(i,0))*(X(0,i)-Y(i,0)));
		}
		return sqrt(d);
	}

	double mdist(mat X,mat Y)
	{
		int M=X.n_rows;
		double sum=0;
		mat temp;
		for(int i=0;i<M;i++)
		{
			temp=Y(i,span::all);
			sum+=dist(X(i,span::all),temp.t());
		}
		return sum/M;
	}

	inline double RandomDirect(int *dim,int *n,int *above,double *trunpt,double *L, mat *z, int j, double times)
	{
		int k;
		double mu,tpz,u,pa,pb,arg;
		double mu2,tpz2,u2,pa2,pb2;
		double curr=0;
		mat Y;
		bool t=true;
		double ratio=0;
	 	mat sol=findSummit(L,j,trunpt,dim); 	
		double prod,s,dis,lu,rb;
		for(int i=0;i<*n;i++)
		{	
			u=(*_runif)();
		//	if(u<0.5){rb=-1;}else{rb=1;}
			dis=0.5*(*_rnorm)();
			double dxc=dist((*z)(i,span(0,j-1)),sol);
			s=1+dis/dxc;
			//cout <<  rb << "\n";
			
			//cout <<  exp(prod) << " ";
			if(dis<1)
			{
				Y=sol.t()*(1-s)+s*(*z)(i,span(0,j-1));
				//	cout << "prop " << Y(i,span::all) << "\n";
				//	cout << "curr " << (*z)(i,span(0,j-1)) << "\n";

				prod=0;
				for(int k=0;k<j;k++)
				{
					prod+=log(pdf(*d,Y(0,k)))-log(pdf(*d,(*z)(i,k)));
				}
					mat J=Jmatrix(sol,(*z)(i,span(0,j-1)),dis,dxc,j);
					prod+=log(det(J));//-log(pdf(*d,dis));

				lu=log((*_runif)());
				if(prod>lu)
				{
					ratio++;
					(*z)(i,span(0,j-1))=Y;					
				}
			}
		}
		return ratio/(*n);
	}
	mat Jmatrix(mat sol, mat X , double d,double dis, int j)
	{
		mat J(j+1,j+1);
		J.fill(0);
		for(int i=0;i<j+1;i++)
		{
			if(i<j)
			{
				for(int k=0;k<i;k++)
				{
					J(i,k)=(sol(i,0)-X(0,i))*d*(X(0,k)-sol(k,0))/pow(dis,3);
					J(k,i)=J(i,k);
				}
				J(i,i)=1+sol(i,0)*d*(X(0,i)-sol(i,0))/pow(dis,3)+d/(pow(dis,2))*(dis+(X(0,i)-sol(i,0))*X(0,i)/dis);
				J(i,j)=-sol(i,0)/dis+X(0,i)/dis;	
			
			
			}
		}
		J(j,j)=1;
		return J;
	}
	mat Rnorm(int j)
	{
		mat prop(1,j);
		for(int i=0;i<j;i++)
		{
			prop(0,i)=(*_rnorm)();
		}
		return prop;

	}

	double GRW(int *dim,int *n,int *above,double *trunpt,double *L, mat *z, int j)
	{
		int ratio=0;
		double rho=1-(double)0.004/pow(j,(double)2/3);
		//cout << "rho" << rho; 
		for(int i=0;i<*n;i++)
		{
			mat R=Rnorm(j);
			mat prop=(*z)(i,span(0,j-1))*rho+sqrt(1-rho*rho)*R;
			//cout <<" prop "  << R;
			//cout <<" prec "  << (*z)(i,span(0,j-1));

			mat thres=Thres(L,prop,j,dim);
			bool t=true;
			for(int k=0;k<j;k++)
			{
				//cout << thres(k,0) << " " << trunpt[k] <<"\n";
				if(thres(k,0)<trunpt[k])
				{
					t=false;				
				}				
			}	
			if(t)
			{
				ratio++;
				(*z)(i,span(0,j-1))=prop;
			}
		}
		return (double)ratio/(*n);

	}

	mat Angle(double *L, int *dim, int j)
	{
		mat A(j,1);
		double sum;
		double sum1;
		double sum2;
		for(int i=0;i<j;i++)
		{
			sum=0;
		 	sum1=0;
			sum2=0;
			for(int k=0;k<j;k++)
			{
				sum+=L[j+k*(*dim)]*L[i+k*(*dim)];
				sum1+=L[j+k*(*dim)]*(L[j+k*(*dim)]);
				sum2+=L[i+k*(*dim)]*(L[i+k*(*dim)]);
			}
			A(i,0)=acos(sum/sqrt(sum1*sum2))*180/PI;
			
		}
		return A;
	}
	mat InfoCone(double *L, int j, int *dim, double* trunpt)
	{	
		mat i(3,1);
		mat O(1,j);
		O.fill(0);
		mat s=findSummit(L,j,trunpt,dim);
		i(0,0)=dist(O,s);	
		mat A=Angle(L,dim,j);
		i(1,0)=A.max();
		i(2,0)=A.min();
		return i;
	}


private:
	RandomG::Random<boost::random::uniform_01<> > *_runif;
	rtnormNC *rt;
	rtnorm *rt2;
	rtNC *rt3;
	Resample *_R;
	boost::math::normal_distribution<> *d;
	RandomG::Random<boost::random::normal_distribution<> > *_rnorm;
	Kernel *_K;
	RandomG::Random<boost::random::gamma_distribution<> > *rgamma;
	double sigma;
	Data<ofstream> *O2;
	Data<ofstream> *O1;
};
